#include "cartographer/rcm/imu/optimize.h"

namespace cartographer
{
    ImuIntegrateOptimizer::ImuIntegrateOptimizer(size_t winSize) : dataSize_(winSize + 1)
    {
        ceresPoses_ = (double **)calloc(dataSize_, sizeof(double *));
        ceresVelocityBias_ = (double **)calloc(dataSize_, sizeof(double *));
        for (size_t i = 0; i < dataSize_; i++)
        {
            ceresPoses_[i] = (double *)calloc(SIZE_POSE, sizeof(double));
            ceresVelocityBias_[i] = (double *)calloc(SIZE_SPEEDBIAS, sizeof(double));
        }
    }

    ImuIntegrateOptimizer::~ImuIntegrateOptimizer()
    {
        for (size_t i = 0; i < dataSize_; i++)
        {
            if (ceresPoses_ && ceresPoses_[i])
                free(ceresPoses_[i]);
            if (ceresVelocityBias_ && ceresVelocityBias_[i])
                free(ceresVelocityBias_[i]);
        }
        if (ceresPoses_)
            free(ceresPoses_);
        if (ceresVelocityBias_)
            free(ceresVelocityBias_);
    }

    void ImuIntegrateOptimizer::Clear()
    {
        while (!poses_.empty())
            poses_.pop_front();
        ClearCeres();
    }

    void ImuIntegrateOptimizer::ClearCeres()
    {
        for (size_t i = 0; i < dataSize_; i++)
        {
            if (ceresPoses_ && ceresPoses_[i])
            {
                for (int p = 0; p < SIZE_POSE; p++)
                {
                    ceresPoses_[i][p] = 0.0;
                }
            }
            if (ceresVelocityBias_ && ceresVelocityBias_[i])
            {
                for (int p = 0; p < SIZE_SPEEDBIAS; p++)
                {
                    ceresVelocityBias_[i][p] = 0.0;
                }
            }
        }
    }

    void ImuIntegrateOptimizer::Push(ImuOptNode::Ptr node)
    {
        while (poses_.size() > dataSize_)
        {
            poses_.pop_front();
        }
        poses_.push_back(node);
    }

    ImuOptNode::Ptr ImuIntegrateOptimizer::GetOptNode(const int64 poseTime)
    {
        for (size_t i = 0; i < poses_.size(); i++)
        {
            if (poses_[i]->time == poseTime)
            {
                return poses_[i];
            }
        }
        return nullptr;
    }

    void ImuIntegrateOptimizer::CheckForOptimize()
    {
        int badIdx = -1;
        for (size_t i = 0; i < poses_.size(); i++)
        {
            if (!poses_[i] || poses_[i]->integrator_ == nullptr || !poses_[i]->integrator_->hasIntegrated)
            {
                badIdx = i;
            }
        }
        if (badIdx > -1)
        {
            for (int p = 0; p < badIdx + 1; p++)
            {
                poses_.pop_front();
            }
        }
    }

    int ImuIntegrateOptimizer::Optimize(ImuOptNode &resNode)
    {
        CheckForOptimize();
        if (poses_.size() < dataSize_)
            return 0;
        ceres::Problem problem;
        // ceres::LossFunction *loss_function;
        // loss_function = new ceres::HuberLoss(1.0);
        // loss_function = new ceres::CauchyLoss(1.0);
        //  11个位姿，11个待优化变量，10个PVQ增量
        for (size_t i = 0; i < dataSize_; i++)
        {
            SetArray(poses_[i], i);
            // 李群上无加法，需要定义四元素的加法
            ceres::LocalParameterization *localParameterize = new PoseUpdater();
            problem.AddParameterBlock(ceresPoses_[i], SIZE_POSE, localParameterize);
            problem.AddParameterBlock(ceresVelocityBias_[i], SIZE_SPEEDBIAS);
        }
        LOG(INFO) << "poseSize " << poses_.size() << " " << dataSize_ << "\n";
        for (size_t i = 0; i < dataSize_; i++)
        {
            LOG(INFO) << " [" << i << "]"
                      << " " << ceresPoses_[i][0] << "," << ceresPoses_[i][1] << "," << ceresPoses_[i][2] << "," << ceresPoses_[i][3] << ","
                      << ceresPoses_[i][4] << "," << ceresPoses_[i][5] << "," << ceresPoses_[i][6] << "=" << ceresVelocityBias_[i][0] << ","
                      << ceresVelocityBias_[i][1] << "," << ceresVelocityBias_[i][2] << "," << ceresVelocityBias_[i][3] << ","
                      << ceresVelocityBias_[i][4] << "," << ceresVelocityBias_[i][5] << "," << ceresVelocityBias_[i][6] << ","
                      << ceresVelocityBias_[i][7] << "," << ceresVelocityBias_[i][8];
        }
        for (size_t m = 0; m < dataSize_ - 1; m++)
        {
            size_t j = m + 1;
            // if (integrators_[j]->sum_dt > 10.0)
            //     continue;
            // IMUFactor *imuFactor = new IMUFactor(integrators_[j]);
            LOG(INFO) << "AddResidualBlock " << m;
            if (poses_[m]->integrator_ == nullptr)
            {
                LOG(INFO) << "AddResidualBlock cannot happend " << m;
            }
            ImuOptEdge *imuEdge = new ImuOptEdge(poses_[m]->integrator_.get());
            problem.AddResidualBlock(imuEdge, NULL, ceresPoses_[m], ceresVelocityBias_[m], ceresPoses_[j], ceresVelocityBias_[j]);
        }
        ceres::Solver::Options options;
        options.linear_solver_type = ceres::DENSE_SCHUR;
        // options.num_threads = 2;
        options.trust_region_strategy_type = ceres::DOGLEG;
        options.max_num_iterations = 50;
        // options.use_explicit_schur_complement = true;
        // options.minimizer_progress_to_stdout = true;
        // options.use_nonmonotonic_steps = true;
        /*if (marginalization_flag == MARGIN_OLD)
            options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
        else
            options.max_solver_time_in_seconds = SOLVER_TIME;*/
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);
        // cout << summary.BriefReport() << endl;
        // ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
        // ROS_DEBUG("solver costs: %f", t_solver.toc());
        Eigen::VectorXd aver = Eigen::VectorXd::Zero(dataSize_);
        for (size_t i = 0; i < dataSize_; i++)
        {
            for (size_t p = 0; p < SIZE_SPEEDBIAS; p++)
            {
                aver(p) += ceresVelocityBias_[i][p];
            }
        }
        aver /= (double)dataSize_;
        resNode.V = Eigen::Vector3d(aver(0), aver(1), aver(2));
        resNode.Ba = Eigen::Vector3d(aver(2), aver(4), aver(5));
        resNode.Bg = Eigen::Vector3d(aver(6), aver(7), aver(8));
        size_t count = 0;
        size_t halfSize = dataSize_ / 2;
        while (!poses_.empty() && count < halfSize)
        {
            count++;
            poses_.pop_front();
        }
        ClearCeres();
        return 1;
    }
} // namespace cartographer
